#include "predict_by_pos.hpp"

namespace sros::location
{
    void predict_by_pos::add_pos(const double &pos)
    {
        std::lock_guard<std::mutex> lck(window_mutex_);
        DataPoint new_point{pos, std::chrono::high_resolution_clock::now()};
        data_window_.push_back(new_point);

        if (data_window_.size() > window_size_) 
        {
            data_window_.pop_front();
        }

        update_cal_();
    }

    void predict_by_pos::update_cal_()
    {
        if (data_window_.size() < 2) 
        {
            return;
        }

        // 计算最新速度（中心差分法）
        auto delta_t = std::chrono::duration_cast<std::chrono::duration<double>>(
            data_window_.back().timestamp - data_window_[data_window_.size()-2].timestamp).count();
        
        double v_curr = 0.0f;
        if (delta_t > 0) 
        {
            v_curr = (data_window_.back().position - data_window_[data_window_.size()-2].position) / delta_t;
        }

        // 计算加速度(需要至少3个点)
        if (data_window_.size() >= 3) 
        {
            auto delta_t_acc = std::chrono::duration_cast<std::chrono::duration<double>>(
                data_window_.back().timestamp - data_window_[data_window_.size()-3].timestamp).count() / 2.0;
            
            if (delta_t_acc > 0)
            {
                double v_prev = (data_window_[data_window_.size()-2].position - 
                                data_window_[data_window_.size()-3].position) / 
                            std::chrono::duration_cast<std::chrono::duration<double>>(
                                data_window_[data_window_.size()-2].timestamp - 
                                data_window_[data_window_.size()-3].timestamp).count();
                
                double acc = (v_curr - v_prev) / delta_t_acc;
                set_U(acc);
            }
        }
        // 在还未计算出acc时，直接通过设置当前预测状态，来避免预测方程计算始终不变
        else 
        {
            decltype(predict_by_acc::param().X_) X(data_window_.back().position, v_curr, 0.0f);
            set_X(X);
        }
    }
}